#include "qhdrcamera.h"
#include <QFile>
#include <QDir>
#include <QDateTime>
#include <QTimer>
#include <math.h>
#include <QMessageBox>
#include <QDebug>

#include <QScopedPointer>
#include <QSharedPointer>

qreal dt[9]={0.0,1.0,0.0,1.0,-4.0,1.0,0.0,1.0,0.0};
//qreal dt[9]={0.0,-1.0,0.0,-1.0,4.0,-1.0,0.0,-1.0,0.0};
//qreal dt[9]={-1.0,-2.0,-1.0,-2.0,12.0,-2.0,-1.0,-2.0,-1.0};
//qreal dt[9]={0.0,0.0,0.0,0.0,4.0,0.0,0.0,0.0,0.0};
QMatrix3x3 QHdrCamera::convol(dt);

QHdrCamera::QHdrCamera(QDeclarativeItem *parent) :
    QNativeCamera(parent)
   ,m_shots(3)
   ,m_exposureRange(2.0)
{
    shot.clear();
}

float QHdrCamera::brightness( const char r, const char g, const char b){
    // 255 * 0.2126 = 54,213
    // 255 * 0.7152 = 182,376
    // 255 * 0.0722 = 18,411
    return (0.2126*(float)r) + (0.7152*(float)g) + (0.0722*(float)b);
}

float QHdrCamera::gray( const float r, const float g, const float b){
    return (0.299*r + 0.587*g + 0.114*b);
}

float QHdrCamera::saturation( const float r, const float g, const float b){
    float max=r;
    float min=r;

    max = ( g > max ) ? g : max;
    max = ( b > max ) ? b : max;

    min = ( g < min ) ? g : min;
    min = ( b < min ) ? b : min;

    return (max-min);
}

void QHdrCamera::laplace(QImage& in, QImage& out){

    float sumR=0.0,sumG=0.0,sumB=0.0;
    int width = in.bytesPerLine();
    int height = in.height();
    unsigned char* inb  = in.bits();
    unsigned char* outb = out.bits();
    int pixelSize = in.depth()/8;
    int lv=0;

    for(int y=0;y<height;y++){
        for(int x=0;x<width-pixelSize;x+=pixelSize){
            sumR=0.0,sumG=0.0,sumB=0.0;
            for(int h=-1;h<2;h++){
                for(int w=-1;w<2;w++){
                    sumR+=(convol(w+1,h+1)* inb[abs(x+w)+(abs(y+h)*width)]);
                    sumG+=(convol(w+1,h+1)* inb[abs(x+w)+1+(abs(y+h)*width)]);
                    sumB+=(convol(w+1,h+1)* inb[abs(x+w)+2+(abs(y+h)*width)]);
                }
            }

            lv = QHdrCamera::gray(sumR,sumG,sumB);
            lv=(lv<256)?lv:255;
            lv=(lv>0)?lv:0;
            if(lv<70)lv=127;

            outb[x+(y*width)] = abs(lv);
            outb[x+1+(y*width)] = abs(lv);
            outb[x+2+(y*width)] = abs(lv);

        }
    }

}

int QHdrCamera::gauss(QImage& in, QImage& out, double sigma) {

    int ton = 0;
    int width  = in.bytesPerLine();
    int height = in.height();
    double l;
    double R=0.0,G=0.0,B=0.0;
    int lv;

    unsigned char* inb  = in.bits();
    unsigned char* outb = out.bits();
    int pixelSize = in.depth()/8;

    float costant = 2*pow((sigma*255),2);
    float gnu = 0.5*255;


    for(int j=0;j<height;j++){
        for(int i=0;i<width-pixelSize;i+=pixelSize){

            l= ( inb[i+(j*width)]-gnu);
            R=exp(-(l*l)/costant);
            l= ( inb[i+1+(j*width)]-gnu);
            G=exp(-(l*l)/costant);
            l= ( inb[i+2+(j*width)]-gnu);
            B=exp(-(l*l)/costant);
            lv=int(R*G*B*255);
            ton+=lv;

            lv= abs(lv);
            lv= (lv<256)?lv:255;

            //outb[i+(j*width)] = R; outb[i+1+(j*width)] = G; outb[i+2+(j*width)] = B;
            outb[i+(j*width)] = lv; outb[i+1+(j*width)] = lv; outb[i+2+(j*width)] = lv;
        }
    }

    return ton;
}

void QHdrCamera::mul(QVector<QImage*>& ins, QImage& out, int k){
    int count = ins.size();
    if( count > 1 ) {
        unsigned char* outb = out.bits();
        int width     = ins[0]->bytesPerLine();
        int height    = ins[0]->height();
        int pixelSize = ins[0]->depth()/8;
        float red=1,green=1,blue=1;

        for(int j=0;j<height;j++){
            for(int i=0;i<width-pixelSize;i+=pixelSize){
                red=1,green=1,blue=1;

                for(k=0; k < count; k++){
                    red   *= ((float)((unsigned char*)ins[k]->bits())[i+(j*width)])/255.0;
                    green *= ((float)((unsigned char*)ins[k]->bits())[i+1+(j*width)])/255.0;
                    blue  *= ((float)((unsigned char*)ins[k]->bits())[i+2+(j*width)])/255.0;
                }
                red*=(255.0+k); green*=(255.0+k); blue*=(255.0+k);

                outb[i+(j*width)] = qBound((unsigned int)0, (unsigned int)red, (unsigned int)255);
                outb[i+1+(j*width)] = qBound((unsigned int)0, (unsigned int)green, (unsigned int)255);
                outb[i+2+(j*width)] = qBound((unsigned int)0, (unsigned int)blue, (unsigned int)255);
            }
        }
    }
}

void QHdrCamera::add(QVector<QImage*>& ins, QImage& out, bool average){
    int count = ins.size();
    if( count > 1 ) {
        unsigned char* outb = out.bits();
        int width     = ins[0]->bytesPerLine();
        int height    = ins[0]->height();
        int pixelSize = ins[0]->depth()/8;
        int red=0,green=0,blue=0;

        int k = 0;

        QColor c1,c2,c3, cmax;

        for(int j=0;j<height;j++){
            for(int i=0;i<width-pixelSize;i+=pixelSize){
                red=0,green=0,blue=0;

                /*if(average) {
                    c1.setRgb( ((unsigned char*)ins[0]->bits())[i+(j*width)],
                               ((unsigned char*)ins[0]->bits())[i+1+(j*width)],
                               ((unsigned char*)ins[0]->bits())[i+2+(j*width)]
                            );
                    c2.setRgb( ((unsigned char*)ins[1]->bits())[i+(j*width)],
                               ((unsigned char*)ins[1]->bits())[i+1+(j*width)],
                               ((unsigned char*)ins[1]->bits())[i+2+(j*width)]
                            );
                    c3.setRgb( ((unsigned char*)ins[2]->bits())[i+(j*width)],
                               ((unsigned char*)ins[2]->bits())[i+1+(j*width)],
                               ((unsigned char*)ins[2]->bits())[i+2+(j*width)]
                            );
                    cmax=c1;
                    /*if( c2.saturation() > cmax.saturation()) cmax=c2;
                    if( c3.saturation() > cmax.saturation()) cmax=c3;*/

                    /*if( c2.lightness() > cmax.lightness()) cmax=c2;
                    if( c3.lightness() > cmax.lightness()) cmax=c3;*/

                    /*red += cmax.red(); green += cmax.green(); blue += cmax.blue();
                }*/

                for(k=0; k < count; k++){
                    red   += ((unsigned char*)ins[k]->bits())[i+(j*width)];
                    green += ((unsigned char*)ins[k]->bits())[i+1+(j*width)];
                    blue  += ((unsigned char*)ins[k]->bits())[i+2+(j*width)];
                }

                if(average) {
                    //red /=(count+1); green /=(count+1); blue /=(count+1);
                    red /=count; green /=count; blue /=count;
                }

                outb[i+(j*width)] = qBound((int)0, red, (int)255);
                outb[i+1+(j*width)] = qBound((int)0, green, (int)255);
                outb[i+2+(j*width)] = qBound((int)0, blue, (int)255);
            }
        }
    }
}

HdrThread::HdrThread(QImage* image,QImage* result){
    this->image = image;
    this->result = result;
}

void HdrThread::run() {
   // QString timeStamp = QDateTime::currentDateTime().toString("yyyyMMddmmsszzz");

    int width  = image->bytesPerLine();
    int height = image->height();
    int pixelSize = image->depth()/8;

    unsigned char* inb  = image->bits();
    unsigned char* outb = result->bits();

    int R = 0; int G = 0; int B = 0;
    float Rl = 0; float Gl = 0; float Bl = 0;

    //float sigma=0.2; float costant = 2*pow((sigma*255),2); float gnu = 0.5*255; float gray=0; float l=0; float lv= 0;
    int subsample=2;

    for(int j=0,sub_y=0;j<height;j++){
        for(int i=0,sub_x=0;i<width-pixelSize;i+=pixelSize){
            //R= inb[(i-(sub_x*pixelSize))+((j-sub_y)*width)]; G= inb[(i-(sub_x*pixelSize))+1+((j-sub_y)*width)]; B= inb[(i-(sub_x*pixelSize))+2+((j-sub_y)*width)];

            R= inb[i+(j*width)];G= inb[i+1+(j*width)]; B= inb[i+2+(j*width)];

            Rl = qBound(0,((inb[i+(j*width)]+inb[(i-(sub_x*pixelSize))+  ((j-sub_y)*width)])/2)   - inb[(i-(sub_x*pixelSize))+  ((j-sub_y)*width)], 255);
            Gl = qBound(0,((inb[i+1+(j*width)]+inb[(i-(sub_x*pixelSize))+1+((j-sub_y)*width)])/2) - inb[(i-(sub_x*pixelSize))+1+((j-sub_y)*width)],255);
            Bl = qBound(0,((inb[i+2+(j*width)]+inb[(i-(sub_x*pixelSize))+2+((j-sub_y)*width)])/2) - inb[(i-(sub_x*pixelSize))+2+((j-sub_y)*width)],255);

          //l= ( (float)inb[i+(j*width)]-gnu);   Rl=exp(-(l*l)/costant);
          //l= ( (float)inb[i+1+(j*width)]-gnu); Gl=exp(-(l*l)/costant);
          //l= ( (float)inb[i+2+(j*width)]-gnu); Bl=exp(-(l*l)/costant);

            outb[i+(j*width)]   = qBound(0,(int)(Rl),255);
            outb[i+1+(j*width)] = qBound(0,(int)(Gl),255);
            outb[i+2+(j*width)] = qBound(0,(int)(Bl),255);

            sub_x++; if( sub_x >= subsample) sub_x=0;
        }
        sub_y++; if( sub_y >= subsample) sub_y=0;
    }
    //result->save("C:\\Data\\Images\\athread-"+timeStamp+".jpg");
}


/*void HdrThread::run() {
    qDebug() << "thread";

    QString timeStamp = QDateTime::currentDateTime().toString("yyyyMMddmmsszzz");

    QImage tGauss(*image); tGauss.fill(0);
    QImage tLaplace(*image); tLaplace.fill(0);
    QImage tImage(*image); tImage.fill(0);

    //image->save("C:\\Data\\Images\\image-passed-"+timeStamp+".jpg");

    QVector<QImage*> vect;
    //QHdrCamera::gauss(*image, tImage);

    QHdrCamera::gauss(*image, tGauss);
    tGauss.save("C:\\Data\\Images\\gauss-"+timeStamp+".jpg");
    QHdrCamera::laplace(tGauss, tLaplace);
    tLaplace.save("C:\\Data\\Images\\laplace-"+timeStamp+".jpg");
    vect.clear(); vect.push_back(&tGauss);vect.push_back(&tLaplace);// vect.push_back(&tGauss);
    QHdrCamera::mul(vect, tImage);
    //QHdrCamera::add(vect, tImage);
    tImage.save("C:\\Data\\Images\\mul-gauss-laplace-"+timeStamp+".jpg");

    vect.clear(); vect.push_back(&tImage); vect.push_back(image);
    QHdrCamera::mul(vect, *result);

    //result->save("C:\\Data\\Images\\thread"+timeStamp+".jpg");
}*/

/*!
  Capture image with the currently set attributes.
*/
void QHdrCamera::captureImage()
{
    qDebug() << "QHdrCamera capture";
    if (!m_cameraImageCapture) {
        return;
    }

    shot.clear();
    tShot.clear();
    vFrame.clear();
    setExposureCompensation(-this->m_exposureRange);
    m_cameraImageCapture->capture();
}

void QHdrCamera::imageAvailable(int id ,QVideoFrame buffer) {

    /*if( !buffer.map(QAbstractVideoBuffer::ReadOnly) ) qDebug() << "Error during mapping";

    QImage* image = new QImage(); image->loadFromData((const uchar *)buffer.bits(), buffer.mappedBytes(), (const char *)"JPEG");
    image->save("C:\\Data\\Images\\shot"+QString::number(shot.size())+".jpg", "jpeg");

    shot.push_back(image);
    //QImage* tImage = new QImage(*image); tShot.push_back(tImage);

    buffer.unmap();*/
    vFrame.push_back(buffer);
    int shot_size = vFrame.count();

    if( shot_size < this->m_shots ){
        setExposureCompensation(-this->m_exposureRange + (shot_size*this->m_exposureRange));
        QTimer::singleShot(0, m_cameraImageCapture, SLOT(capture()));
    } else {
        setExposureCompensation(0);
        QTimer::singleShot(0, this, SLOT(process()));
    }
}

void QHdrCamera::imageCaptured ( int id, const QImage & preview ) {
    qDebug() << "QHdrCamera::imageCaptured id:" << QString::number(id);
    qDebug() << "preview.width()" << QString::number(preview.width());
    qDebug() << "preview.height()" << QString::number(preview.height());
    qDebug() << "preview.bytesPerLine()" << QString::number(preview.bytesPerLine());

    //preview.save("C:\\Data\\Images\\pippo.jpg", "jpeg");
}

void QHdrCamera::process() {

    qDebug() << "Start processing";
    emit started();

    for( int i = 0 ; i < vFrame.size(); i++ ){
        if( !vFrame[i].map(QAbstractVideoBuffer::ReadOnly) ) qDebug() << "Error during mapping";

            QImage* image = new QImage(); image->loadFromData((const uchar *)vFrame[i].bits(), vFrame[i].mappedBytes(), (const char *)"JPEG");
            //image->save("C:\\Data\\Images\\shot"+QString::number(shot.size())+".jpg", "jpeg");

            shot.push_back(image);

            vFrame[i].unmap();
    }

    QImage result(*shot[0]);// result.fill(0);
    int sz = shot.size();
    for( int i = 0 ; i < sz ; i++){
        QImage* tImage = new QImage(shot[0]->width(), shot[0]->height(), shot[0]->format()); tShot.push_back(tImage);
    }

    QVector<QImage*> vect;

   /* HdrThread* t1 = new HdrThread(shot[0], tShot[0]);
    HdrThread* t2 = new HdrThread(shot[1], tShot[1]);
    HdrThread* t3 = new HdrThread(shot[2], tShot[2]);

    qDebug() << "starting threads";
    QThreadPool::globalInstance()->start(t1);
        //QThreadPool::globalInstance()->waitForDone();
    QThreadPool::globalInstance()->start(t2);
       // QThreadPool::globalInstance()->waitForDone();
    QThreadPool::globalInstance()->start(t3);

    QThreadPool::globalInstance()->waitForDone();
    qDebug() << "All threads finished"; qDebug() << "------------";
*/
    //tImage1.save("C:\\Data\\Images\\aImage1.jpg"); tImage2.save("C:\\Data\\Images\\aImage2.jpg");  tImage3.save("C:\\Data\\Images\\aImage3.jpg");

    vect.clear(); vect.push_back(shot[0]); vect.push_back(shot[1]); vect.push_back(shot[2]);
    //vect.clear(); vect.push_back(&tImage1); vect.push_back(&tImage2); vect.push_back(&tImage3);
    emit merging();
    QHdrCamera::add(vect, result);

    /*vect.clear(); vect.push_back(tShot[0]); vect.push_back(tShot[1]); vect.push_back(tShot[2]); vect.push_back(&result);
    QHdrCamera::add(vect, result,false);
    vect.clear();*/

    QString timeStamp = QDateTime::currentDateTime().toString("yyyyMMddmmss");
    QString savePath = QDir::toNativeSeparators("C:\\Data\\Images\\hdr-"+timeStamp+".jpg");

    if( result.isNull()) qDebug() << "result image null";
    if( result.save(savePath, "jpeg") ) {
        //i_preview.load(savePath);
        i_preview = result;
        qDebug() << "Result image saved";
    } else {
        qDebug() << "Error saving result image to:"<<savePath;
    }

    QString saveOrigPath = QDir::toNativeSeparators("C:/Data/Images/hdr-original-"+timeStamp+".jpg");
    shot[1]->save(saveOrigPath);

    emit finished(savePath,saveOrigPath);

    shot.clear(); tShot.clear();
}

unsigned short QHdrCamera::shots(){
    return m_shots;
}

void QHdrCamera::setShots(unsigned short value){
    this->m_shots = value;
    emit shotsChanged(value);
}


qreal QHdrCamera::exposureRange(){
    return this->m_exposureRange;
}

void QHdrCamera::setExposureRange(qreal value){
    this->m_exposureRange=value;
    emit exposureRangeChanged(value);
}
